/* gm_sim.c -- Simulador gráfico de futebol de robôs
 * 
 * Copyright (C) 2008, Gustavo Campos Martins <gcm.martins@gmail.com>
 * GEAR - Grupo de Estudos Avançados em Robótica
 * <http://www.sel.eesc.usp.br/gear>
 * 
 * This program is free software: you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation, either version 3 of the License, or
 * (at your option) any later version.
 * 
 * This program is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 * 
 * You should have received a copy of the GNU General Public License
 * along with this program.  If not, see <http://www.gnu.org/licenses/>.
 *
 */

#include <math.h>
#include <stdio.h>
#include <stdlib.h>

#include "gm_sim.h"
#include "gm_sim_utils.h"
#include "gm_sim_glutstuff.h"
#include "gm_sim_physics.h"

cpSpace *space;
cpBody *robot[NUM_ROB];
cpBody *ball;
cpBody *staticBody;

int kick[NUM_ROB];
int dribble[NUM_ROB];
int placar_a, placar_b;

/** Inicializa o simulador.
 *
 * @return Ponteiro para a estrutura que representa o simulador,
 * essa estrutura é passada como parâmetro em outras funções.
 */
void
gm_sim_run()
{
	/* inicializa a física do simulador */
	gm_sim_phys_init();

	/* inicializa o glut */
	gm_sim_glut_init();
}

/** Atualiza um vetor com dos dados de posições da bola e jogadores.
 * 
 * @param id_robot Número de identificação do robô de referência.
 *
 * @param *vector Ponteiro para o vetor que será atualizado com os dados do 
 * jogo (posição da bola e dos robôs e angulo dos robôs). <b>Este vetor deve 
 * estar alocado com pelo menos 32 posições antes da função ser chamada.</b>
 * Disposição dos dados:
 * 	|BALL_X|BALL_Y|POS1_X|POS1_Y|POS1_A|POS2_X|...|
 */
void 
gm_sim_get_field( float * vector )
{
	gm_sim_phys_get_ballvel(vector);

	int i, p;
	float tmp[3];
	for (i = 0; i < NUM_ROB*3; i += 3) {
		p = i/3;
		gm_sim_phys_get_robotpos(tmp, p);
		gm_sim_phys_get_robotang(&tmp[2], p);
		vector[2+i] = tmp[0];
		vector[3+i] = tmp[1];
		vector[4+i] = tmp[2];
	}
}


/** Atualiza um vetor com dados sobre as dimensões do mundo.
 *
 * @param *vector Ponteiro para o vetor que será atualizado com os dados do 
 * mundo. <b>Este vetor deve estar alocado com pelo menos 28 posições antes 
 * da função ser chamada.</b>
 */
void 
gm_sim_get_world( float * vector )
{
	/* ponto (x0, y0) do mundo */
	vector[0] = WORLD_BL.x;
	vector[1] = WORLD_BL.y;

	/* ponto (x1, y1) do mundo */
	vector[2] = WORLD_BR.x;
	vector[3] = WORLD_BR.y;

	/* ponto (x2, y2) do mundo */
	vector[4] = WORLD_UR.x;
	vector[5] = WORLD_UR.y;

	/* ponto (x3, y3) do mundo */
	vector[6] = WORLD_UL.x;
	vector[7] = WORLD_UL.y;

	/* ponto (x0, y0) do campo */
	vector[8] = FIELD_BL.x;
	vector[9] = FIELD_BL.y;

	/* ponto (x1, y1) do campo */
	vector[10] = FIELD_BR.x;
	vector[11] = FIELD_BR.y;

	/* ponto (x2, y2) do campo */
	vector[12] = FIELD_UR.x;
	vector[13] = FIELD_UR.y;

	/* ponto (x3, y3) do campo */
	vector[14] = FIELD_UL.x;
	vector[15] = FIELD_UL.y;

	/* meio do campo superior */
	vector[16] = (FIELD_UR.x + FIELD_UL.x)/2.0;
	vector[17] = FIELD_UL.y;
	
	/* meio do campo inferior */
	vector[18] = vector[16];
	vector[19] = FIELD_BL.y;

	/* Gol 0 */
	vector[20] = GOALOUR_U.x;
	vector[21] = GOALOUR_U.y;
	vector[22] = GOALOUR_B.x;
	vector[23] = GOALOUR_B.y;

	/* Gol 1 */
	vector[24] = GOALOPP_U.x;
	vector[25] = GOALOPP_U.y;
	vector[26] = GOALOPP_B.x;
	vector[27] = GOALOPP_B.y;
}

/** Seta a velocidade de um robô.
 *
 * @param id_robot O número do robô (de 0 a 4 robôs do nosso time e de 5 a
 * 9 são os robôs do time adversário).
 *
 * @param vx Velocidade no eixo X em m/s.
 *
 * @param vy Velocidade no eixo Y em m/s.
 *
 * @param va Velocidade angular em rad/s.
 */
void 
gm_sim_robot_set_vel( int id_robot, 
		      float vx, 
		      float vy, 
		      float va )
{
	float tmp[2] = {vx*VEL2MPS, vy*VEL2MPS};
	gm_sim_phys_set_robotvel(tmp, id_robot);
	gm_sim_phys_set_robotw(va, id_robot);
}

/** Faz um robô chutar.
 *
 * @param id_robot O número do robô (de 0 a 4 robôs do nosso time e de 5 a
 * 9 são os robôs do time adversário).
 *
 * @param level Nível (força) do chute.
 */
void 
gm_sim_robot_kick( int id_robot, 
		   int level )
{
	gm_sim_phys_set_kick(level, id_robot);
}

/** Ativa ou desativa o rolinho de um determinado robô.
 *
 * @param id_robot O número do robô (de 0 a 4 robôs do nosso time e de 5 a
 * 9 são os robôs do time adversário).
 *
 * @param enable Se for 0 desativa o rolinho, caso contrário ativa o rolinho.
 */
void 
gm_sim_robot_dribble_enable( int id_robot, 
			     int enable )
{
	gm_sim_phys_set_dribble( enable, id_robot );
}

/**  
 * Destrói o simulador.
 */
void 
gm_sim_destroy( void )
{
}

